/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package core;

import java.awt.Point;
import java.util.LinkedList;

public class BreadthFirstSearch{

    public Point pstart, pfin;
    
    LinkedList <Truck> queue = new LinkedList();
    LinkedList <Truck> visited = new LinkedList();
    LinkedList <Point> Parent = new LinkedList();
    LinkedList <Point> Child = new LinkedList();
    LinkedList <Point> path = new LinkedList(); 
           
    public BreadthFirstSearch(Point p1, Point p2){
        pstart = new Point(p1.x,p1.y);
        pfin = new Point(p2.x,p2.y);
    }
    
    public boolean isPathFound(Truck t){
//        if (((t.xtruck == pfin.x-1) && (t.ytruck == pfin.y))||((t.xtruck == pfin.x) && (t.ytruck == pfin.y-1))
//            ||((t.xtruck == pfin.x+1) && (t.ytruck == pfin.y))||((t.xtruck == pfin.x) && (t.ytruck == pfin.y+1)))
//            return true;
//        else
//            return false;
        if ((t.xtruck == pfin.x)&&(t.ytruck == pfin.y))
            return true;
        else
            return false;
    }
    
    public boolean isVisited(Truck t)
    {
        for(Truck t1 : visited)
        {
            if(t.equals(t1))
                return true;
        }
        return false;
    }
    
    public boolean isVisitedParent(Point p)
    {
        for(Point p1 : Parent)
        {
            if(p.equals(p1))
                return true;
        }
        return false;
    }
    
    public boolean isVisitedChild(Point p)
    {
        for(Point p1 : Child)
        {
            if(p.equals(p1))
                return true;
        }
        return false;
    }
    
    public void printPoint(int x, int y){
        System.out.println(x+","+y);
    }
    
    public void BFS(Point p){
        pstart = p;
        Truck t = new Truck(p.x,p.y);
        
        queue.add(t); //memasukkan pf ke dalam antrian
        visited.add(t); //memasukkan pf ke dalam list visited
        
       if ((isPathFound(t))){
            queue.clear();
            visited.clear();
            return;
       }
       
        while (!(queue.isEmpty()))
        {
            Truck up = new Truck(t.xtruck,t.ytruck);
            Truck down = new Truck(t.xtruck,t.ytruck);
            Truck left = new Truck(t.xtruck,t.ytruck);
            Truck right = new Truck(t.xtruck,t.ytruck);
            queue.getFirst().copyTruck(up);
            queue.getFirst().copyTruck(down);
            queue.getFirst().copyTruck(left);
            queue.getFirst().copyTruck(right);
            queue.removeFirst();
            
            if(up.isUp()){   
                up.goUp();
                if(isPathFound(up)){
//                    if (!isVisitedParent(new Point(up.xtruck+1,up.ytruck)) && !isVisitedParent(new Point(up.xtruck,up.ytruck))){
                    Parent.add(new Point(up.xtruck+1,up.ytruck));
                    Child.add(new Point(up.xtruck,up.ytruck));
                    visited.add(up);
                    queue.clear();
                    visited.clear();
                    System.out.println("Path found.");
                    return;
                }
                if (!isVisited(up)){
                    if (!isVisitedParent(new Point(up.xtruck+1,up.ytruck)) && !isVisitedParent(new Point(up.xtruck,up.ytruck))){                    
                    Parent.add(new Point(up.xtruck+1,up.ytruck));
                    Child.add(new Point(up.xtruck,up.ytruck));}
                    visited.add(up);
                    queue.add(up);
                }
            }

            if(down.isDown()){
                down.goDown();
                if(isPathFound(down)){
//                    if (!isVisitedParent(new Point(up.xtruck-1,up.ytruck)) && !isVisitedParent(new Point(up.xtruck,up.ytruck))){
                    Parent.add(new Point(down.xtruck-1,down.ytruck));
                    Child.add(new Point(down.xtruck,down.ytruck));
                    visited.add(down);
                    queue.clear();
                    visited.clear();
                    System.out.println("Path found.");
                    return;
                }
                if (!isVisited(down)){
  //                  if (!isVisitedParent(new Point(up.xtruck-1,up.ytruck)) && !isVisitedParent(new Point(up.xtruck,up.ytruck))){
                    Parent.add(new Point(down.xtruck-1,down.ytruck));
                    Child.add(new Point(down.xtruck,down.ytruck));
                    visited.add(down);
                    queue.add(down);
                }
            }

            if(left.isLeft()){
                left.goLeft();
                if(isPathFound(left)){
//                    if (!isVisitedParent(new Point(up.xtruck,up.ytruck+1)) && !isVisitedParent(new Point(up.xtruck,up.ytruck))){
                    Parent.add(new Point(left.xtruck,left.ytruck+1));
                    Child.add(new Point(left.xtruck,left.ytruck));
                    visited.add(left);
                    queue.clear();
                    visited.clear();
                    System.out.println("Path found.");
                    return;
                }
                if (!isVisited(left)){
//                    if (!isVisitedParent(new Point(up.xtruck,up.ytruck+1)) && !isVisitedParent(new Point(up.xtruck,up.ytruck))){                    
                    Parent.add(new Point(left.xtruck,left.ytruck+1));
                    Child.add(new Point(left.xtruck,left.ytruck));
                    visited.add(left);
                    queue.add(left);
                }
            }

            if(right.isRight()){
                right.goRight();
                if(isPathFound(right)){
//                    if (!isVisitedParent(new Point(up.xtruck,up.ytruck-1)) && !isVisitedParent(new Point(up.xtruck,up.ytruck))){
                    Parent.add(new Point(right.xtruck,right.ytruck-1));
                    Child.add(new Point(right.xtruck,right.ytruck));
                    visited.add(right);
                    queue.clear();
                    visited.clear();
                    System.out.println("Path found.");
                    return;
                }
                if (!isVisited(right)){
//                    if (!isVisitedParent(new Point(up.xtruck,up.ytruck+1)) && !isVisitedParent(new Point(up.xtruck,up.ytruck))){
                    Parent.add(new Point(right.xtruck,right.ytruck-1));
                    Child.add(new Point(right.xtruck,right.ytruck));
                    visited.add(right);
                    queue.add(right);
                }
            }

        }
    }
    
    public void findPath(){
        LinkedList <Point> p = new LinkedList();
        for(int i=0;i<Parent.size();i++){
            System.out.println("parent["+i+"]="+Parent.get(i).x+","+Parent.get(i).y);
            System.out.println("child["+i+"]="+Child.get(i).x+","+Child.get(i).y);
        }
//        System.out.println("parent size:"+Parent.size());
//        System.out.println("child size:"+Child.size());
        p.add(new Point(Child.get(Child.size()-1).x,Child.get(Child.size()-1).y));
        int pathIndex = Child.size()-1;
        for (int i=Parent.size()-1;i>0;i--){
            if (Child.get(i).equals(Parent.get(pathIndex))){
                p.add(new Point(Parent.get(pathIndex).x,Parent.get(pathIndex).y));
                pathIndex = i;
            }
        }
        
        p.add(new Point(Parent.get(pathIndex).x,Parent.get(pathIndex).y));
        int idx = 0;
        for(int i=p.size()-1;i>0;i--){    
                //System.out.println("p : ("+p.getLast().x+","+p.getLast().y+")");
                path.add(idx, p.getLast());
                //System.out.println("path : ("+path.get(idx).x+","+path.get(idx).y+")");
                idx++;
                p.removeLast();
        }
        path.addLast(pfin);
    }
    
    public void printPath(){
        for(int i=0;i<path.size();i++){
            System.out.println("path : ("+path.get(i).x+","+path.get(i).y+")");
        }
    }
    
    public static void main(String[] args){
        Parser.parse("input5x5.txt");
        Map.debugPrint();
        BreadthFirstSearch pf = new BreadthFirstSearch(new Point(2,4),new Point(0,4));     
        pf.BFS(pf.pstart);
        pf.findPath(); pf.printPath();
    }

}
